Reasoning and Descision Making under Uncertainty

Portfolio Exam 2

Sensor Fusion - Kalman and Particle Filter

Task P2.1

Kalman Filter

Realize an implementation of the Kalman Filter in a programming language of your choice for a simulation of the ball-throwing example from the lecture slides. The task of your Kalman Filter is to estimate the position and velocity vector of the ball only from the observed erroneous positions over time.

Your implementation shall be flexible in the sense that it can handle the following variations:

ˆ Simulate the trajectory of a ball with the parameters launch position (especially the height above an imaginary ground), launch speed and launch angle of the ball.

ˆ Simulate the observation of the ball position (x, y as shown in the slides). The estimated ball position shall be subject to uncertainty and it shall be possible to parameterize this uncertainty. In addition, the time span between two observations shall be variable and the observations shall be able to drop out completely over a certain period of time.

ˆ The initial parameters of the Kalman Filter shall be adaptable.

ˆ The normally distributed noise on transition and observation should be adjustable. This means that the covariance matrices R and Q shall be set as a parameter in the Kalman Filter.

Trajectory of a ball with the Launch Paramaters

import numpy as np
import matplotlib.pyplot as plt
import seaborn as sns

# Defining the launch variables
Initial_X = 0
Initial_Y = 0  # height from the ground in meters
Initial_Speed = 80.0  # Initial velocity of the ball in m/s
Launch_Angle = 45.0  # launch angle in degrees
g = 9.81  # gravity in m/s^2 

# Convert the launch angle to radians
Launch_angle_rad = np.deg2rad(Launch_Angle)  # launch angle in radians

# Calculate the total time of flight
time_flight = (2 * Initial_Speed * np.sin(Launch_angle_rad)) / g

# Number of samples
num = 50

# Generating time points
delta_t = np.linspace(0, time_flight, num)  # time points 

# Calculating the x and y coordinates
x_real = Initial_X + Initial_Speed * np.cos(Launch_angle_rad) * delta_t + np.random.normal(0, 0.1, num)
y_real = Initial_Y + Initial_Speed * np.sin(Launch_angle_rad) * delta_t - 0.5 * g * delta_t**2 + np.random.normal(0, 0.1, num)

# Calculating the horizontal and vertical components of velocity
v_x = Initial_Speed * np.cos(Launch_angle_rad)  # same (No change in Vx)
v_y = Initial_Speed * np.sin(Launch_angle_rad) - g * delta_t  # Becomes zero after reaching peak

# Calculating the resultant velocity
velocity = np.sqrt(v_x**2 + v_y**2)  # Resultant magnitude of velocity

plt.figure(figsize=(12, 12))

# Subplot for Y vs X (trajectory)
plt.subplot(2, 1, 1)
plt.plot(x_real, y_real, 'o:b', label=f'Position of ball')  # Projectile path
plt.title('Projectile motion of a ball')
plt.xlabel('Horizontal Distance (X) in meters')
plt.ylabel('Height (Y) in meters')
plt.grid(True)
plt.ylim(bottom=0)
plt.legend()

# Subplot for velocity vs distance
plt.subplot(2, 1, 2)
plt.plot(x_real, velocity, 'r', label=f'Velocity of ball')  # Velocity vs distance
plt.title('Velocity vs  Distance for the ball')
plt.xlabel(' Distance (X) in meters')
plt.ylabel('Velocity (m/s)')
plt.grid(True)
plt.legend()

plt.tight_layout()
plt.show()

Objective:

Implementation of the Kalman filter from equations provided in lecture slides and use of the Kalman filter from the filterpy.kalman library. Comparison between both implementations.

Using Kalman Filter from filterpy.kalman

import numpy as np
import matplotlib.pyplot as plt
import plotly.graph_objects as go
from scipy.linalg import block_diag
from filterpy.kalman import KalmanFilter
from filterpy.common import Q_discrete_white_noise

# Define launch variables
Initial_X = 0
Initial_Y = 0
Initial_Speed = 12


# Define time step
dt = 0.01

# Gravity in m/s^2 
g = 9.81 

Launch_Angle = 45.0  # launch angle in degrees
Launch_angle_rad = np.deg2rad(Launch_Angle)
x_vel = Initial_Speed * np.cos(Launch_angle_rad)
y_vel = Initial_Speed * np.sin(Launch_angle_rad)

# Total time until it reaches ground level y = 0
total_time = (3 * Initial_Speed * np.sin(Launch_angle_rad)) / g

# Define initial state vector
q_0 = np.array([Initial_X, Initial_Y, x_vel, y_vel])  

# Define initial covariance matrix
P_0 = np.eye(4) * 10000

# Define transition matrix F
F = np.array([[1., 0, dt, 0],
              [0, 1., 0, dt],
              [0, 0, 1., 0],
              [0, 0, 0, 1.]])

# Control input matrix
B = np.array([[0, 0, 0],
              [0, 0.5*dt, 0],
              [0, 0, 0],
              [0, dt, 0]])

# Control vector
u = np.array([0, -g, 0])

# Define observation matrix (measurement matrix)
H = np.array([[1., 0, 0, 0],
              [0, 1., 0, 0]])

# Define process noise covariance
Q = block_diag(Q_discrete_white_noise(dim=2, dt=dt, var=0.05),
               Q_discrete_white_noise(dim=2, dt=dt, var=0.05))

# Define measurement noise covariance
R = np.array([[0.01, 0],
              [0, 0.1]])


# Initialize the Kalman Filter
kf = KalmanFilter(dim_x=4, dim_z=2)
kf.x = q_0
kf.B = B
kf.F = F
kf.H = H
kf.Q = Q
kf.R = R
kf.P = P_0

# Generate noisy observations
t = np.arange(0, total_time, dt)
n = len(t)

z = np.zeros((n, 2))  # observed position
# Define the start and end time of the dropout 
dropout_start = 0.3
dropout_end = 0.5
for i in range(n):
    # Check if the current time is in the dropout period 
    if dropout_start <= t[i] <= dropout_end:
        continue
    # all the x axes observations
    z[i, 0] = q_0[0] + q_0[2]*t[i] + np.random.normal(0, np.sqrt(R[0, 0]))
    # all the y axes observations
    z[i, 1] = q_0[1] + q_0[3]*t[i] - 0.5*g*t[i]**2 + np.random.normal(0, np.sqrt(R[1, 1]))

# Real Curve
p = np.zeros((n, 2))
for i in range(n):
    # all the x axes observations
    p[i, 0] = q_0[0] + q_0[2]*t[i]
    # all the y axes observations
    p[i, 1] = q_0[1] + q_0[3]*t[i] - 0.5*g*t[i]**2

P_est = np.zeros((n, 4, 4))
# Use Kalman Filter for estimation
x_est = np.zeros((n, 4))  # estimated state


for i in range(n):
    kf.predict(B=B, u=u)
    if dropout_start <= t[i] <= dropout_end:
        # Increase measurement noise during dropout period
        kf.R = np.array([[1000., 0],
                         [0, 1000.]])
    else:
        kf.R = R
        kf.update(z[i])
    x_est[i] = kf.x
    P_est[i] = kf.P

# Plot true, observed and estimated position
fig = go.Figure()
# noisy observations
fig.add_trace(go.Scatter(x=z[:, 0], y=z[:, 1],mode='markers',name='Noisy observations of the ball position at various time steps',marker=dict(symbol='x')))
# estimated positions by Kalman filter
fig.add_trace(go.Scatter(x=x_est[:, 0], y=x_est[:, 1],mode='lines',name='Estimated position by Kalman filter',line=dict(color='red')))
# real flight curve
fig.add_trace(go.Scatter(x=p[:, 0], y=p[:, 1],mode='lines',name='Real flight curve',line=dict(color='blue')))
# estimated position + covariance
fig.add_trace(go.Scatter(x=x_est[:, 0] + np.sqrt(P_est[:, 0, 0]), y=x_est[:, 1] + np.sqrt(P_est[:, 1, 1]),mode='lines',name='Estimated position + covariance',line=dict(color='green')))
# estimated position - covariance
fig.add_trace(go.Scatter(x=x_est[:, 0] - np.sqrt(P_est[:, 0, 0]), y=x_est[:, 1] - np.sqrt(P_est[:, 1, 1]),mode='lines',name='Estimated position - covariance',line=dict(dash='dash', color='green')))
# shaded area for dropout period
fig.add_shape(type="rect",x0=dropout_start * x_vel, y0=0, x1=dropout_end * x_vel, y1=max(z[:, 1]),fillcolor="blue", opacity=0.3, line_width=0)
fig.add_trace(go.Scatter(x=[None], y=[None],mode='markers',name='Dropout period',marker=dict(color='red', opacity=0.3)))
fig.update_layout(title='True and Estimated Position',xaxis_title='Distance (m)',yaxis_title='Height (m)',yaxis=dict(range=[0, max(z[:, 1]) + 1]),legend=dict(orientation="h"),showlegend=True)

# Show plot
fig.show()

# Plot true and estimated position
plt.figure(figsize=(12, 5))

plt.plot(x_est[:, 0], x_est[:, 1], '.',color='red', label='Estimated position by Kalman filter')
plt.plot(p[:, 0], p[:, 1], 'b', label="Real flight curve")

plt.axvspan(dropout_start * x_vel, dropout_end * x_vel, color='blue', alpha=0.3, label='Dropout period')

plt.xlabel('Distance (m)')
plt.ylabel('Height (m)')
plt.ylim(bottom=0)
plt.legend(loc='best')
plt.grid(True)
plt.show()

Implementation of kalman fliter using equations given in lecture slides

1. $\mu'_t = A_t \mu_{t-1} + B_t a_{t-1}$

State Prediction: Predicts the next state based on the previous state and control input

2. $\Sigma'_t = A_t \Sigma_{t-1} A_t^T + Q_t$

Covariance Prediction: Predicts the covariance of the state estimate

3. $K_t = \Sigma'_t C_t^T (C_t \Sigma'_t C_t^T + R_t)^{-1}$

Kalman Gain: Determines the weight given to the new measurement

4. $\mu_t = \mu'_t + K_t (o_t - C_t \mu'_t)$

State Update: Updates the state estimate with the new measurement

5. $\Sigma_t = (I - K_t C_t) \Sigma'_t$

Covariance Update: Updates the covariance of the state estimate

Explanation of variables:

1. $A_t$ (or $F$): State transition matrix, which describes how the state evolves from one time step to the next in the absence of control input.
2. $B_t$(or $B$): Control input matrix, which describes how the control input affects the state.
3. $a_{t-1}$ (or $u$): Control vector, which represents external influences on the state.
4. $\Sigma_t$ (or $P$): Covariance matrix, which represents the uncertainty in the state estimate.
5. $Q_t$ (or $Q$): Process noise covariance matrix, which represents the uncertainty in the process model.
6. $C_t$ (or $H$): Observation matrix, which maps the true state space into the observed space.
7. $R_t$ (or $R$): Measurement noise covariance matrix, which represents the uncertainty in the measurements.
8. $o_t$ (or $z$): Measurement vector, which represents the observed values.
9. $I$: Identity matrix, which is used to update the covariance matrix.

I took variable names as per standard filter.py library

# Define launch variables
Initial_X = 0
Initial_Y = 0
Initial_Speed = 12
# Define time step
dt = 0.01
# Gravity in m/s^2 
g = 9.81 
Launch_Angle = 45.0  # launch angle in degrees
Launch_angle_rad = np.deg2rad(Launch_Angle)
x_vel = Initial_Speed * np.cos(Launch_angle_rad)
y_vel = Initial_Speed * np.sin(Launch_angle_rad)
# Total time until it reaches ground level y = 0
total_time = (3 * Initial_Speed * np.sin(Launch_angle_rad)) / g
# Define initial state vector
q_0 = np.array([Initial_X, Initial_Y, x_vel, y_vel])  
# Define initial covariance matrix
P_0 = np.eye(4) * 10000
# Define transition matrix F
F = np.array([[1., 0, dt, 0],
              [0, 1., 0, dt],
              [0, 0, 1., 0],
              [0, 0, 0, 1.]])
# Control input matrix
B = np.array([[0, 0, 0],
              [0, 0.5*dt, 0],
              [0, 0, 0],
              [0, dt, 0]])
# Control vector
u = np.array([0, -g, 0])
# Define observation matrix (measurement matrix)
H = np.array([[1., 0, 0, 0],
              [0, 1., 0, 0]])
# Define process noise covariance
Q = block_diag(Q_discrete_white_noise(dim=2, dt=dt, var=0.05),
               Q_discrete_white_noise(dim=2, dt=dt, var=0.05))
# Define measurement noise covariance
R = np.array([[0.01, 0],
              [0, 0.1]])

# Generate noisy observations
t = np.arange(0, total_time, dt)
n = len(t)

# Real Curve
p = np.zeros((n, 2))
for i in range(n):
    # all the x axes observations
    p[i, 0] = q_0[0] + q_0[2]*t[i]
    # all the y axes observations
    p[i, 1] = q_0[1] + q_0[3]*t[i] - 0.5*g*t[i]**2

z = np.zeros((n, 2))  # observed position
# Define the start and end time of the dropout 
dropout_start = 0.3
dropout_end = 0.5

# Initialize arrays to store results
x_est = np.zeros((n, 4))
P_est = np.zeros((n, 4, 4))
vel_est = np.zeros(n)
x_obs = np.zeros(n)
y_obs = np.zeros(n)


# Define the Kalman filter function
def kalman_Filter_using_equation(F, H, B, u, q_prior, P_prior, z, Q, R):
    # Predicting the next state
    q_pred = np.dot(F, q_prior) + np.dot(B, u)
    # Predicting the error covariance 
    P_pred = np.dot(F, np.dot(P_prior, F.transpose())) + Q
    # Computing the Kalman Gain
    K = np.dot(P_pred, np.dot(H.transpose(), np.linalg.inv(np.dot(H, np.dot(P_pred, H.transpose())) + R)))
    # Updating the state estimate (posterior)
    q_post = q_pred + np.dot(K, (z - np.dot(H, q_pred)))
    # Updating the error covariance (posterior)
    P_post = np.dot(np.identity(K.shape[0]) - np.dot(K, H), P_pred)
    return q_post, P_post

# Simulate the trajectory and generate noisy observations
for i in range(n):
    x_true = Initial_X + x_vel * t[i]
    y_true = Initial_Y + y_vel * t[i] - 0.5 * g * t[i]**2
    if y_true < 0:
        y_true = 0
    
    z[i] = np.array([x_true, y_true]) + np.random.multivariate_normal([0, 0], R)
    x_obs[i], y_obs[i] = z[i]
    
    if (dropout_start <= t[i] <= dropout_end):
        # Increasing measurement noise during dropout period
        R_current = np.array([[1000., 0],
                              [0, 1000.]])
        z_current = np.array([0, 0])
    else:
        R_current = R
        z_current = z[i]
    
    q_0, P_0 = kalman_Filter_using_equation(F, H, B, u, q_0, P_0, z_current, Q, R_current)
    x_est[i] = q_0
    P_est[i] = P_0
    # Calculate velocity
    vel_est[i] = np.sqrt(q_0[2]**2 + q_0[3]**2)

# Calculate true velocity
vel_true = np.sqrt(x_vel**2 + (y_vel - g*t)**2)

# Plot true, observed and estimated position
fig = go.Figure()

# noisy observations
fig.add_trace(go.Scatter(x=z[:, 0], y=z[:, 1],mode='markers',name='Noisy observations of the ball position at various time steps',marker=dict(symbol='x')))
# estimated positions by Kalman filter
fig.add_trace(go.Scatter(x=x_est[:, 0], y=x_est[:, 1],mode='lines',name='Estimated position by Kalman filter',line=dict(color='red')))
# real flight curve
fig.add_trace(go.Scatter(x=p[:, 0], y=p[:, 1],mode='lines',name='Real flight curve',line=dict(color='blue')))
# estimated position + covariance
fig.add_trace(go.Scatter(x=x_est[:, 0] + np.sqrt(P_est[:, 0, 0]), y=x_est[:, 1] + np.sqrt(P_est[:, 1, 1]),mode='lines',name='Estimated position + covariance',line=dict(color='green')))
# estimated position - covariance
fig.add_trace(go.Scatter(x=x_est[:, 0] - np.sqrt(P_est[:, 0, 0]), y=x_est[:, 1] - np.sqrt(P_est[:, 1, 1]),mode='lines',name='Estimated position - covariance',line=dict(dash='dash', color='green')))
# shaded area for dropout period
fig.add_shape(type="rect",x0=dropout_start * x_vel, y0=0, x1=dropout_end * x_vel, y1=max(z[:, 1]),fillcolor="blue", opacity=0.3, line_width=0)
fig.add_trace(go.Scatter(x=[None], y=[None],mode='markers',name='Dropout period',marker=dict(color='blue', opacity=0.2)))
fig.update_layout(title='True and Estimated Position',xaxis_title='Distance (m)',yaxis_title='Height (m)',yaxis=dict(range=[0, max(z[:, 1]) + 1]),legend=dict(orientation="h"),showlegend=True)
# Show plot
fig.show()

# Ploting true and estimated position
plt.figure(figsize=(12, 5))

plt.plot(x_est[:, 0], x_est[:, 1], '.',color='red', label='Estimated position by Kalman filter')
plt.plot(p[:, 0], p[:, 1], 'b', label="Real flight curve")

plt.axvspan(dropout_start * x_vel, dropout_end * x_vel, color='blue', alpha=0.3, label='Dropout period')

plt.xlabel('Distance (m)')
plt.ylabel('Height (m)')
plt.ylim(bottom=0)
plt.legend(loc='best')
plt.grid(True)
plt.show()


# Ploting velocity variation with distance
plt.figure(figsize=(12, 5))
sns.scatterplot(x=x_obs, y=vel_true, color='blue', label='Velocity Observed', marker='o', alpha=0.7)
plt.plot(x_est[:, 0], vel_est, color='purple', linestyle='--', label='Velocity Predicted')
plt.xlabel('Distance')
plt.ylabel('Velocity')
plt.title('Velocity Variation with Distance')
plt.legend()
plt.grid(True)
plt.show()

True and Estimated Position:

1. Noisy observations: The blue crosses represent the noisy observations of the ball's position at various time steps.
2. Real flight curve: The blue line shows the true trajectory of the ball.
3. Estimated position by Kalman filter: The red line indicates the position estimates provided by the Kalman filter.
4. Estimated position ± covariance: The green lines(line for Plus and dashed line for minus) illustrate the estimated position plus and minus the covariance, showing the uncertainty in the estimates.
5. Dropout period: The shaded blue region highlights the time interval where there is an increase in measurement noise, simulating a sensor dropout.

Estimated vs Real Position:

1. Estimated position by Kalman filter: The red dots depict the estimated positions provided by the Kalman filter.
2. Real flight curve: The blue line represents the actual trajectory of the ball.
3. Dropout period: The shaded blue region highlights the period with increased measurement noise, causing a temporary loss of accurate position data.

Velocity Variation with Distance:

1. Velocity Observed: The blue dots show the observed velocity at various distances, affected by noise.
2. Velocity Predicted: The purple dashed line represents the velocity estimates provided by the Kalman filter. It accounts for the noise and gives a smoother estimation compared to the observed velocity.

These plots demonstrate the effectiveness of the Kalman filter in estimating the true state of a system, even in the presence of noise and periods of high measurement uncertainty.

THE END

Name: THARUN KUMAR KORINE PALLI

Matriculation Number: 5123708

Email: tharunkumar.korinepalli@study.thws.de